#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
static const rclcpp::Logger LOGGER = rclcpp::get_logger("mtc_tutorial");
#include <sensor_msgs/msg/joint_state.h>
/*
这里是一个简单的接收四元数，然后plan和excute
然后发送给
*/

class EegoalNode : public rclcpp::Node
{
public:
    EegoalNode(std::string name) : Node(name)
    {
        auto qos = rclcpp::QoS(1).reliable().transient_local();
        sub_goal = this->create_subscription<geometry_msgs::msg::Pose>(
            "ee_goal_pose",
            qos,
            std::bind(
                &EegoalNode::ee_pose_callback,
                this,
                std::placeholders::_1));
        plan_goal_trajectory = this->create_publisher<moveit_msgs::msg::RobotTrajectory>(
            "plan_goal_trajectory", 10);
        timer_ = this->create_wall_timer(
            std::chrono::seconds(1),
            std::bind(&EegoalNode::Publisher_goal_trajectory, this));

        pub_plan = this->create_publisher<std_msgs::msg::String>(
            "plan_topic",
            10);

        pub_excute = this->create_publisher<std_msgs::msg::String>(
            "excute_topic",
            10);
    }
    void InitialMoveGroup()
    {
        // 这行代码会自动去寻找模型，然后加载
        move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(shared_from_this(), "arm");
        // move_group_->setEndEffectorLink("ee_link");
        // move_group_->setStartStateToCurrentState();
    }

private:
    std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
    rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr sub_goal;

    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<moveit_msgs::msg::RobotTrajectory>::SharedPtr plan_goal_trajectory;

    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_plan;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_excute;

    moveit_msgs::msg::RobotTrajectory trajectory;
    // 接受到位姿就开始解算//也没有考虑障碍物之类的
    void ee_pose_callback(const geometry_msgs::msg::Pose::SharedPtr msg)
    {
        if (!move_group_)
        {
            RCLCPP_ERROR(this->get_logger(), "initial is fail");
            return;
        }
        RCLCPP_INFO(this->get_logger(), "ING>>>>");

        move_group_->setStartStateToCurrentState();

        move_group_->setPoseTarget(*msg);

        auto plan_msg = std_msgs::msg::String();
        plan_msg.data = "planning!!!";
        pub_plan->publish(plan_msg);

        moveit::planning_interface::MoveGroupInterface::Plan plan;
        bool success = (move_group_->plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
        if (success)
        {
            trajectory = plan.trajectory_;

            RCLCPP_INFO(this->get_logger(), "Planning successful, executing the plan");
            plan_msg.data = "Successful";
            pub_plan->publish(plan_msg);
            plan_goal_trajectory->publish(trajectory);


            // Update the goal state in RViz
            move_group_->setStartStateToCurrentState();
            RCLCPP_INFO(this->get_logger(), "planning is completed");
        }
        else
        {
            RCLCPP_WARN(this->get_logger(), "Planning failed");
            plan_msg.data = "Failed";
            pub_plan->publish(plan_msg);
        }
    };
    void Publisher_goal_trajectory()
    {
        if (!trajectory.joint_trajectory.points.empty())
        {
            plan_goal_trajectory->publish(trajectory);
            RCLCPP_INFO(this->get_logger(), "this is a trajectory of planning ");
        }
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);

    auto node = std::make_shared<EegoalNode>("ee_goal_node");

    // Initialize MoveGroupInterface after the node is fully constructed
    node->InitialMoveGroup();

    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}